#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import re
import os

import matplotlib.pyplot as plt
'''
import logging
from loggercfg import loggerCfgConsoleHandler
from loggercfg import loggerCfgFileHandler
from loggercfg import loggerCfgFileAndConsoleHandler

loggerc = loggerCfgConsoleHandler()
loggerf = loggerCfgFileHandler()
loggerfc = loggerCfgFileAndConsoleHandler()
'''

'''
* classname    : pid
* para()       :
* return       : 
* discription  : 
* author       : zhongmingsen
* date         : 2023-06-14 
'''
class pid(object) :
    def __init__(self, Kp, Ki, Kd) :

        self.Kp = Kp  #比例系数
        self.Ki = Ki  #微分系数
        self.Kd = Kd  #积分系数

        self.limitTarget = 0    #输出目标限制
        self.limitOut = 0       #输出调节量限制
        self.limitKiErr = 0  #输出积分调节量限制

        self.deltauk = 0
        self.target = 0
        self.feedback = 0 #实际测量值
        self.uk = 0       #设定的目标值
        self.ek0 = 0      #当前误差
        self.ek1 = 0      #前一个误差
        self.ek2 = 0      #前期一个误差
        self.sumEk = 0    #误差的总和，这个就是误差的积分

    def setValue(self, limitTarget, limitOut, limitKiErr) :
        self.limitTarget = limitTarget
        self.limitOut = limitOut
        self.limitKiErr = limitKiErr

    '''
    参数整定找最佳，从小到大顺序查
    先是比例后积分，最后再把微分加
    曲线振荡很频繁，比例度盘要放大
    曲线漂浮绕大湾，比例度盘往小扳
    曲线偏离回复慢，积分时间往下降
    曲线波动周期长，积分时间再加长
    曲线振荡频率快，先把微分降下来
    动差大来波动慢，微分时间应加长
    理想曲线两个波, 前高后低4比1
    一看二调多分析，调节质量不会低
    '''

    '''
    * functionname : positonal
    * para()       :
    * return       : 
    * discription  : 
    位置式PID
    u(k) = Kp * e(k) + Ki * sum[e(0)~e(k)] + Kd * (e(k) - e(k-1))
    = Kp * e(k) + Ki * sum[e(0)~e(k)] + Kd * (e(k) - e(k-1))
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def positionalCal(self, target, feedback): 
        try :
            if target >  self.limitTarget :
                target = self.limitTarget
            elif  target < 0 - self.limitTarget :
                target = 0 - self.limitTarget

            self.target = target
            self.feedback = feedback
            self.ek2 = self.ek1
            self.ek1 = self.ek0
            self.ek0 = self.target - self.feedback

            k = 0
            if abs(self.ek0) < self.limitKiErr :
                k = 1
            self.sumEk = self.sumEk + self.ek0 * k

            self.uk = self.Kp * self.ek0 + self.Ki * self.sumEk + self.Kd * (self.ek0 - self.ek1)
            if self.uk > self.limitOut :   
                self.uk = self.limitOut
            elif self.uk < (0 - self.limitOut) : 
                self.uk = 0 - self.limitOut
            return self.uk
        except :
            print("positonalCal error!")
            return 0

    '''
    * functionname : incrementalCal
    * para()       :
    * return       : 
    * discription  : 
    增量式PID
    Δu(k) = u(k) - u(k-1)
        = Kp * (e(k) - e(k-1)) + Ki * e(k) + Kd * [e(k) - 2e(k-1) + e(k-2)]
        = (Kp + Ki + Kd) * e(k)  -
          (Kp + 2 * Kd) * e(k-1) + 
          Kd * e(k-2)
        = A * e(k) - B * e(k-1) + C * e(k-2)  
    其中系数A、B、C是系数系数,为:
    A = Kp + Ki + Kd
    B = Kp + 2 * Kd
    C = Kd

    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def incrementalCal(self, target, feedback): 
        try :
            if target >  self.limitTarget :
                target = self.limitTarget
            elif  target < 0 - self.limitTarget :
                target = 0 - self.limitTarget
            
            self.target = target
            self.feedback = feedback
            self.ek2 = self.ek1
            self.ek1 = self.ek0
            self.ek0 = self.target - self.feedback

            A = self.Kp + self.Ki + self.Kd
            B = self.Kp + 2 * self.Kd
            C = self.Kd

            self.deltauk = A * self.ek0 - B * self.ek1 + C * self.ek2

            if self.deltauk > self.limitOut :
                self.deltauk = self.limitOut
            elif self.deltauk < (0 - self.limitOut) :
                self.deltauk = 0 - self.limitOut
            #print(target, feedback, A, B, C, self.ek0, self.ek1, self.deltauk)
            return self.deltauk
        except :
            print("incrementalCal error!")
            return 0
        
        '''
    * functionname : noPidLimitOutCal
    * para()       :
    * return       : 
    * discription  : 
    * author       : zhongmingsen
    * date         : 2023-06-14 
    '''
    def noPidLimitOutCal(self, target, feedback): 
        try :
            if target >  self.limitTarget :
                target = self.limitTarget
            elif  target < 0 - self.limitTarget :
                target = 0 - self.limitTarget
            
            if target - feedback > self.limitOut :
                feedback = int(feedback + self.limitOut)
            elif target - feedback < 0 - self.limitOut :
                feedback = int(feedback - self.limitOut)
            else :
                feedback = int(target)

            self.target = target
            self.feedback = feedback
            
            return feedback
        except :
            print("incrementalCal error!")
            return 0
#------------------------------------------------------
#----------------------以下是测试code--------------------
#------------------------------------------------------  
def pidTest(): 
    mypid = pid(0.8, 100*20, 50)
    mypid.setValue(50*1000, 50*1000, 1000*1000, 990*1000)
    try :
        target = 0
        deltauk = 0
        for i in range(100) :
            #target = mypid.positonalCal(mypid.target, mypid.feedback + target)
            target = mypid.incrementalCal(mypid.target, mypid.feedback + target)
            
            print('feedback :', mypid.feedback)
            #print('target :', mypid.target)
            print('*******:',target, '-----', i)
    except :
        print ('------logger_test error-------------')

'''
* function name : main
* para()        : 
* return        : 
* description   : 用于测试
* author        : zhongmingsen
* date          : 2023.4.1
'''                          
def main(argv=None):
    if argv is None:
        argv = sys.argv
        if len(argv) < 1:
            print('please set dbc file name!')
        else:                                           
            pidTest()
            print('get dbc signal tabel success!')   

if __name__ == "__main__":
    sys.exit(main())


